#include <fstream>   // file I/O
#include <iomanip>   // format manipulation
#include <string>
#include <list>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include "./classes/matriz.h"
#include "./classes/tga.h"

using namespace std;


int main()
{
	cout << "TP2 - Laboratorio Métodos Numéricos"<<endl<<endl;
	
	//Cargamos la mascara de la esfera
	TGA mascara("../esfera_mask.tga");
	
	//Cargamos 3 imágenes de las esferas (se pueden cargar otras 3 imágenes de esferas distintas):
	TGA img1("../Imagenes/esfera/esfera_0.tga");
	TGA img2("../Imagenes/esfera/esfera_1.tga");
	TGA img3("../Imagenes/esfera/esfera_2.tga");
	
	cout << "Calibración"<<endl;
	//1. Calibración
	//Calculamos la direccion de iluminacion de cada imagen:
				
		//Buscamos los pixeles máximos de cada imagen (p1, p2 y p3):
		pair <double,double> p1 = img1.getMax();
		pair <double,double> p2 = img2.getMax();
		pair <double,double> p3 = img3.getMax(); //cómo se obtienen los máximos? Cómo los buscamos? 
				
		//Cuando elegimos el punto máximo su normal va a ser igual a (-) la dirección de iluminación.
		//Con el punto máximo ya tenemos S_x y S_y. Falta S_z = - RaizCuadrada (r*r + S_x*S_x + S_y*S-Y)
		
		//Obtenemos el radio:
		double radio = mascara.getRadio();
		cout<<"radio: "<<radio<<endl;
		
		pair< int, int > centro = mascara.getCentro(radio);
		cout<<"centro de la esfera: "<<"("<<centro.first<<","<<centro.second<<")"<<endl;
		
		p1.first = p1.first - centro.first;
		p2.first = p2.first - centro.first;
		p3.first = p3.first - centro.first;
		
		p1.second = centro.second - p1.second;
		p2.second = centro.second - p2.second;
		p3.second = centro.second - p3.second;
				
		//Calculamos S_z para cada imagen:
		double z1 = - sqrt(radio*radio - p1.first*p1.first - p1.second*p1.second);
		cout<<"z1: "<<z1<<endl;
		double z2 = - sqrt(radio*radio - p2.first*p2.first - p2.second*p2.second);
		cout<<"z2: "<<z2<<endl;
		double z3 = - sqrt(radio*radio - p3.first*p3.first - p3.second*p3.second);
		cout<<"z3: "<<z3<<endl;
		//Tenemos los vectores S (orientaciones distintas de las imágenes - Direccion de iluminación) y los normalizamos:				
		double normaS1 = sqrt(p1.first*p1.first + p1.second*p1.second + z1*z1);
		cout<<"normaS1: "<<normaS1<<endl;
		
		double* s1 = new double[3];
		s1[0] = p1.first/normaS1;
		s1[1] = p1.second/normaS1;
		s1[2] = z1/normaS1; 
				
		double normaS2 = sqrt(p2.first*p2.first + p2.second*p2.second + z2*z2);
		double* s2 = new double[3];
		s2[0] = p2.first/normaS2;
		s2[1] = p2.second/normaS2;
		s2[2] = z2/normaS2;
		
		double normaS3 = sqrt(p3.first*p3.first + p3.second*p3.second + z3*z3);
		double* s3 = new double[3];
		s3[0] = p3.first/normaS3;
		s3[1] = p3.second/normaS3;
		s3[2] = z3/normaS3;		
				
	cout<<"Construcción Campo Normal y Normalización"<<endl;
	//2. Construcción Campo Normal
	//Como ya tenemos identificados las direcciones de iluminación de cada una de las imágenes (s1, s2 y s3), debemos
	//determinar el campo normal m (Ecuación (5)):
		
	//Hacemos la matriz de la ecuación (5):
	int filas = 3;
	int columnas = 3;
	
	Matriz L(filas,columnas);
	Matriz U(filas,columnas);
	
	//Creamos la matriz de los vectores S para la Ecuacion 5:
	Matriz matrizS(filas,columnas); 
		
	//Llenamos la matriz de 3 x 3 con los S	
    for(int j=1;j<=columnas;j++) {
		double valor1 = s1[j];
		double valor2 = s2[j];
		double valor3 = s3[j];
    
        matrizS.setPos(1,j,valor1);            		
		matrizS.setPos(2,j,valor2);
		matrizS.setPos(3,j,valor3);
	}
	
	//Obtenemos en L y en U la factorizacion LU de matrix
	matrizS.getLU(L,U);
	//Tenemos el ancho y alto de las imagenes
	int altoImagen = mascara.height;
	int anchoImagen = mascara.width;

	//Creamos una matriz del tamaño de la imagen donde vamos a poner los resultados de la construccion del campo Normal:
	Matriz campoNormalMatriz(altoImagen, anchoImagen, 3);
	//En esta matriz en cada coordenada vamos aponer un array de tamaño 3, para guardar en cada uno los 3 m que corresponden
	//a cada pixel.
	
	
	//Como en L tenemos la composicion L de la matrizS, eso significa que estan los multiplicadores que se utilizaron para triangular la matriz
	//Asi que esos multiplicadores lo vamos a usar para saber como queda la matrizS si se hace eliminacion Gaussiana
	cout<<"L: "<<endl;
	cout<<L<<endl;
	//L =						matrizS = 
	// 		( 1   0   0 )				( sx1  sy1  sz1 ):( I1 )
	// 		( A1  1   0 )				( sx2  sy2  sz2 ):( I2 )
	// 		( A2  A3  1 )				( sx3  sy3  sz3 ):( I3 )
	
	//Las filas son de matrizS:	
	//F2 = F2 -  A1.F1 , para poner un 0 en sx2, luego:
	//F3 = F3 -  A2.F1 , para poner un 0 en sx3, luego:
	//F3 = F3 -  A3.F2 , para poner un 0 en sy3 , nos queda la matrizS triangular inferior. Y podemos resolver el sistema.
	
	//Esto lo tenemos que hacer para cada coordenada de las imagenes.
	
	//Buscamos los A en la matriz L:
	double A1 = L.getId(2,1);
	double A2 = L.getId(3,1);
	double A3 = L.getId(3,2);
	
	cout<<"A1: "<<A1<<endl;
	cout<<"A2: "<<A2<<endl;
	cout<<"A3: "<<A3<<endl;
		
	//Ahora tenemos que poner sx2, sx3, sy3 en cero y modificar sy2, sz2, sz3, I2 e I3
	double sy2 = matrizS.getId(2,2) - matrizS.getId(1,2)*A1; //sy2 = sy2 - sy1.A1
	double sz2 = matrizS.getId(2,3) - matrizS.getId(1,3)*A1; //sz2 = sz2 - sz1.A1 
	double sz3 = matrizS.getId(3,3) - matrizS.getId(1,3)*A2 - sz2*A3; //sz3 = sz3 - sz1.A2 - sz2.A3
	
	//Actualizamos los valores en la matrizS:
	matrizS.setPos(2,2,sy2);
	matrizS.setPos(2,3,sz2);
	matrizS.setPos(3,3,sz3);
	matrizS.setPos(2,1,0); //Ponemos 0 para que quede triangular inferior.
	matrizS.setPos(3,1,0);
	matrizS.setPos(3,2,0);
	
	cout<<"matrizS: "<<endl;
	cout<<matrizS<<endl;
	//Ahora tenemos que ver los I y guardar los m en la matrizCampoNormal:
	// (sx1 sy1  sz1)(mx):(I1)
	// (0   sy2  sz3)(my):(I2)
	// (0    0   sz3)(mz):(I3)
	
	// => mx = (sy1*my + sz1*mz - I1) / sx1
	// => my = (sz3*mz - I2) / sy2
	// => mz = I3 / sz3
	double mx;
	double my;
	double mz;
	double* vectorM = new double[3];
	
	for(int i = 1; i <= altoImagen; i++) {
        for(int j = 1; j <= anchoImagen; j++){
			double I1 = img1.getPixel(i,j)[0]; //Usamos sólo el canal Red (como va a ser la esfera, tomamos este canal y decimos que esta en escala de grises)
			double I2 = img2.getPixel(i,j)[0];
			double I3 = img3.getPixel(i,j)[0];
			
			//Si sz3, sy2 o matrizS.getId(1,1) es muy chico generan mucho error.
			mx = I3 / sz3;
			my = (sz3*mz - I2)/sy2;
			mz = (matrizS.getId(1,2)*my + matrizS.getId(1,3)*mz - I1) / matrizS.getId(1,1);	
			
			//Normalizamos los m:
			double normaM = sqrt(mx*mx+my*my+mz*mz);
			//Puede ser chica la normaM y generar errores.
			mx = mx/normaM;
			my = my/normaM;
			mz = mz/normaM;
			
			vectorM[0] = mx;
			vectorM[1] = my;
			vectorM[2] = mz;
			
			//Agregamos el vector a la matriz:
			campoNormalMatriz.setPosCampoN(i,j,vectorM);	
		}
	}

	//Finalmente para cada pixel de la imagen tenemos en (x,y) y el vectorM en la matriz campoNormalMatriz.		
    cout<<"Hallar la profundidad"<<endl;    
	
	
	

		
    return 0;
}
